#include "board.h"

void flash_wait_ready(uint32_t Timeout)
{
  /* Wait for the FLASH operation to complete by polling on BUSY flag to be reset.
     Even if the FLASH operation fails, the BUSY flag will be reset and an error
     flag will be set */

    uint32_t tickstart = HAL_GetTick();
    uint32_t error;

    while (__HAL_FLASH_GET_FLAG(FLASH_FLAG_BSY))
    {
        if ((HAL_GetTick() - tickstart) > Timeout)
        {
            break;
        }
    }

    /* Check FLASH operation error flags */
    error = (FLASH->SR & FLASH_FLAG_SR_ERRORS);
    /* Clear error programming flags */
    __HAL_FLASH_CLEAR_FLAG(error);

    /* Check FLASH End of Operation flag  */
    if (__HAL_FLASH_GET_FLAG(FLASH_FLAG_EOP))
    {
        /* Clear FLASH End of Operation pending bit */
        __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP);
    }
}

bool flash_unlock(void)
{
  	if (READ_BIT(FLASH->CR, FLASH_CR_LOCK) != 0U)
  	{
    	/* Authorize the FLASH Registers access */
    	WRITE_REG(FLASH->KEYR, FLASH_KEY1);
    	WRITE_REG(FLASH->KEYR, FLASH_KEY2);
	    /* verify Flash is unlocked */
    	if (READ_BIT(FLASH->CR, FLASH_CR_LOCK) != 0U)
    	{
      		return false; 
    	}
  	}
  	return true;
}

bool flash_lock(void)
{
  	/* Set the LOCK Bit to lock the FLASH Registers access */
  	SET_BIT(FLASH->CR, FLASH_CR_LOCK);

  	/* verify Flash is locked */
  	if (READ_BIT(FLASH->CR, FLASH_CR_LOCK) != 0U)
  	{
    	return true;
  	}

  	return false;
}

int flash_erase_page(uint32_t Page)
{
    uint32_t Banks = FLASH_BANK_1;
    flash_wait_ready((uint32_t)FLASH_TIMEOUT_VALUE);

	flash_unlock();
	
#if defined (FLASH_OPTR_DBANK)
    if (READ_BIT(FLASH->OPTR, FLASH_OPTR_DBANK) == 0U)
    {
        CLEAR_BIT(FLASH->CR, FLASH_CR_BKER);
    }
    else
    {
        if ((Banks & FLASH_BANK_1) != 0U)
        {
            CLEAR_BIT(FLASH->CR, FLASH_CR_BKER);
        }
        else
        {
             SET_BIT(FLASH->CR, FLASH_CR_BKER);
        }
    }
#else
    /* Prevent unused argument(s) compilation warning */
    UNUSED(Banks);
#endif /* FLASH_OPTR_DBANK */

    /* Proceed to erase the page */
    MODIFY_REG(FLASH->CR, FLASH_CR_PNB, ((Page & 0xFFU) << FLASH_CR_PNB_Pos));
    SET_BIT(FLASH->CR, FLASH_CR_PER);
    SET_BIT(FLASH->CR, FLASH_CR_STRT);
    flash_wait_ready((uint32_t)FLASH_TIMEOUT_VALUE);
    /* If the erase operation is completed, disable the PER Bit */
    CLEAR_BIT(FLASH->CR, (FLASH_CR_PER | FLASH_CR_PNB));

	flash_lock();

    return 1;
}

static uint32_t page_from_address(uint32_t Address)
{
	return (Address - CONFIG_FLASH_ADDR) / FLASH_PAGE_SIZE;
}

int flash_erase(u32 address) {
    return flash_erase_page(page_from_address(address));
}

static void flash_write_dword(uint32_t Address, uint64_t Data)
{
    flash_wait_ready((uint32_t)FLASH_TIMEOUT_VALUE);
    /* Set PG bit */
    SET_BIT(FLASH->CR, FLASH_CR_PG);

    /* Program first word */
    *(uint32_t *)Address = (uint32_t)Data;

    /* Barrier to ensure programming is performed in 2 steps, in right order
    (independently of compiler optimization behavior) */
    __ISB();

    /* Program second word */
    *(uint32_t *)(Address + 4U) = (uint32_t)(Data >> 32U);
    flash_wait_ready((uint32_t)FLASH_TIMEOUT_VALUE);
    CLEAR_BIT(FLASH->CR, FLASH_CR_PG);
}

int flash_write_page(u32 target_addr, uint8_t *data, u32 length, bool erase) {
	int offset = 0;
	if (target_addr & 0x7) {
		return 0;
	}
    if (erase && !flash_erase(target_addr)) {
        return 0;
    }
	flash_unlock();
    for (; length >= 8; data += 8, offset += 8, length -=8) {
        u64 dword;
        memcpy(&dword, data, 8);
        flash_write_dword(target_addr + offset, dword);
    }
	if (length > 0) {
		u64 last_dword = 0;
		memcpy(&last_dword, data, length);
		flash_write_dword(target_addr + offset, last_dword);
	}
	flash_lock();
    return 1;
}

bool flash_iap_need_erase(u32 address) {
    return true;
}
